
#include "main.h"


#include <stm32f10x.h>
#include <stm32f10x_gpio.h>
#include <stm32f10x_rcc.h>
#include <stm32f10x_tim.h>
#include <misc.h>

//volatile uint16_t ADC2ConvertedValue;

//#define MSG_SIZE BOARD_STRUCTURE_SIZE

// We use it just for reading sensors data.
#define MSG_SIZE SENSOR_RAW_DATA_SIZE
uint8_t msg[SENSOR_RAW_DATA_SIZE];

int main( void)
{
  /*TODO: Check size of data packet. */
    uint8_t rec_buff[48]; /* Buffer for data packet from PC. */

    /* control_pid_est_t board ={0}; */
    /* it is for reading sensors data only */
    sensor_raw_data_t board     = {0};
    sensor_PID_data_t board_PID = {0};
    sensor_t gyro;
    /* Setup board PID pointer. */
    board.PID = &board_PID;

    init_all();

    init_dma(msg,MSG_SIZE);

    /* Init accel measurement. */
    initAccel();

    /* Detect and init Gyro sensor. */
    //TODO: add checking for error
    l3g4200dDetect(&gyro);
    gyro.init();

    /* Init mag measurement. */
    initMag();

    /* Init pressure measurement. */
    initPressure();
    init_PID();
    /* Start main interupt. */
    //We temporary disable main service interrupt
    ////////////////// start_main_interupt_function(&board,&gyro);
    start_main_interupt_function_sensor(&board,&gyro);

    while( 1)
    {

        /* LED. */
        GPIO_SetBits( GPIOB, GPIO_Pin_1);
        //GPIO_SetBits( GPIOA, GPIO_Pin_6);
        //GPIO_SetBits( GPIOA, GPIO_Pin_7);
        if(!ReceiveDataPacket(rec_buff, 48,  COM1))
        {
          sys_delay(3);
        }
        sys_delay(30);
        GPIO_ResetBits( GPIOB, GPIO_Pin_1);
        //GPIO_ResetBits( GPIOA, GPIO_Pin_6);
        //GPIO_ToggleBits( GPIOA, GPIO_Pin_7);

        sys_delay(30);
#if 0

        size_board = sizeof(board);
//        printf("baro = %d , pressure = %d\n", board.Gyro_raw_XY,size_board);
        board.BoardSystemTime = (uint32_t) read_system_time();
//       printf("Direct time = %d, ", board.BoardSystemTime);
        // Get acceleration data
        readAccel();
        board.Acceleration_raw_X = rawAccel[XAXIS].value;
        board.Acceleration_raw_Y = rawAccel[YAXIS].value;
        board.Acceleration_raw_Z = rawAccel[ZAXIS].value;
        // Get Gyro data
        gyro.read(gyro_raw_data);
        board.Gyro_raw_XY = gyro_raw_data[0];
        board.Gyro_raw_XZ = gyro_raw_data[1];
        board.Gyro_raw_YZ = gyro_raw_data[2];

        // Get magnitometer data
        readMag();
        sys_delay(20);
        board.Magnetometer_raw_X = rawMag[XAXIS].value;
        board.Magnetometer_raw_Y = rawMag[YAXIS].value;
        board.Magnetometer_raw_Z = rawMag[ZAXIS].value;
       // user_serialisation_api(msg,&board, sizeof(msg));
       sensors_raw_data_serialisation_api(msg,&board, sizeof(msg));
#endif

    }




#if 0
    while( 1)
    {
        /* LED. */
//        GPIO_SetBits( GPIOB, GPIO_Pin_1);
        sys_delay(50);
        //delay(50000);

 //       GPIO_ResetBits( GPIOB, GPIO_Pin_1);
        sys_delay(50);
        //delay(50000);
//        baro = ( (float)(get_adc_value()*3.32) )/4096.0;
//        pressure = ((baro/3.32)+0.095 )/0.009;
//        pressure = fir(pressure);
        printf("baro = %d , pressure = %d\n", board.Gyro_raw_XY,board.Acceleration_raw_X);
        //read_char_UART1();
 //       read_char_UARTx(COM1);
    }
#endif

#if 0
  while( 1)
  {
    GPIO_SetBits( GPIOB, GPIO_Pin_1);
//    sys_delay(100);
    GPIO_ResetBits( GPIOB, GPIO_Pin_1);
//    sys_delay(100);
//  printf("Duty = %d,Capture=%d,TIM4Freq=%d\n", Duty, Capture, TIM4Freq);
    printf("Ch1 = %d,Ch2 = %d,Ch3 = %d,Ch4 = %d,Ch5 = %d,Ch6 = %d,\n",
                                                get_ppm_channel_value(0),
                                                get_ppm_channel_value(1),
                                                get_ppm_channel_value(2),
                                                get_ppm_channel_value(3),
                                                get_ppm_channel_value(4),
                                                get_ppm_channel_value(5)
                                              );
    a = (uint16_t)get_ppm_channel_value(0);
    if(a < 1400)
    {
      a=900;
    }
    else
    {
      a = 2000;
    }
    timer2_PWM_duty_CH1(a);

//    timer2_PWM_duty_CH1(a);
//    timer2_PWM_duty_CH2(a);
//    timer2_PWM_duty_CH3(a);
//    timer2_PWM_duty_CH4(a);
  }
#endif
}

void init_all( void)
{
  /* Init priority group. */
  NVIC_init();
  /* Init GPIO. */
  gpio_init();
  /* Init UART modules. */
  uart_init();
  /* Init TIMER modules. */
  timer_init();
  /* Init ADC. */
  //  adc_init();
  /* Init I2C. */
  i2c_init();

#if 0
  {
    /* Init gyro measurement. */
    sensor_t gyro;
    int16_t  gyro_data[3] = {0,0,0};

    l3g4200dDetect(&gyro);
    gyro.init();
    while(1)
    {
        gyro.read(gyro_data);

        printf("X = %d, Y = %d, Z = %d \n",gyro_data[0],
                                           gyro_data[1],
                                           gyro_data[2]
                                        );
        sys_delay(20);
    }
  }
#endif

#if 0
  {
    /* Init mag measurement. */
    initMag();
    while(1)
    {
        readMag();
        printf("X = %d, Y = %d, Z = %d \n",rawMag[XAXIS].value ,
                                           rawMag[YAXIS].value,
                                           rawMag[ZAXIS].value
                                        );
        sys_delay(20);
    }
  }
#endif

#if 0
  { float x_a;
    /* Init accel measurement. */
   initAccel();

    while(1)
    {
      readAccel();
      x_a = fir(rawAccel[XAXIS].value);

      x_a = median_average(x_a);

      printf("X = %f, Y = %f, Z = %f \n",x_a - accelRTBias[XAXIS],
                                         (float)rawAccel[YAXIS].value - accelRTBias[YAXIS],
                                         (float)rawAccel[ZAXIS].value - accelRTBias[ZAXIS]
                                        );
      sys_delay(20);
    }
  }
#endif


#if 0
  {
    /* Init pressure measurement. */
    initPressure();
    while(1)
    {
      read_altitude();
      pressureAlt = fir(pressureAlt);
      printf("alt = %f, temper = %d \n", pressureAlt,((b5+8)/16));
    }
  }
#endif



#if 0
  {
    /* Init pressure measurement. */
    initPressure();
    while(1)
    {
      read_altitude();
      pressureAlt = fir(pressureAlt);
      printf("alt = %f, temper = %d \n", pressureAlt,((b5+8)/16));
    }
  }
#endif

  return;
}

/******************************************************************************/
/*            STM32F10x Peripherals Interrupt Handlers                        */
/******************************************************************************/
/**
  * @brief  This function handles TIM2 global interrupt request.
  * @param  None
  * @retval None
  */
void TIM2_IRQHandler(void)
{
  if(TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
  {
    TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
  }
  if(TIM_GetITStatus(TIM2, TIM_IT_CC1) == SET)
  {
    TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
  }
  if(TIM_GetITStatus(TIM2, TIM_IT_CC2) == SET)
  {
    TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
  }
  if(TIM_GetITStatus(TIM2, TIM_IT_CC3) == SET)
  {
    TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
  }
  if(TIM_GetITStatus(TIM2, TIM_IT_CC4) == SET)
  {
    TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
  }
}
/**
  * @brief  This function handles TIM3 global interrupt request.
  * @param  None
  * @retval None
  */
void TIM3_IRQHandler(void)
{
  if(TIM_GetITStatus(TIM3, TIM_IT_Update) == SET)
  {
    /* Clear TIM3 Capture compare interrupt pending bit */
    TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
  }
  if(TIM_GetITStatus(TIM3, TIM_IT_CC1) == SET)
  {
    TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
  }
  if(TIM_GetITStatus(TIM3, TIM_IT_CC2) == SET)
  {
    TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
  }
  if(TIM_GetITStatus(TIM3, TIM_IT_CC3) == SET)
  {
    TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
  }
  if(TIM_GetITStatus(TIM3, TIM_IT_CC4) == SET)
  {
    TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
  }
}


#if 0
/**
  * @brief  This function handles TIM4 global interrupt request.
  * @param  None
  * @retval None
  */
void TIM4_IRQHandler(void)
{
  volatile static int32_t c_overflow=0;
  volatile static uint32_t flag = 0;

  if(TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
  {
    /* Clear TIM3 Capture compare interrupt pending bit */
    TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
    c_overflow++;
  }
  if(TIM_GetITStatus(TIM4, TIM_IT_CC1) == SET)
  {
    TIM_ClearITPendingBit(TIM4, TIM_IT_CC1);
  }
  if(TIM_GetITStatus(TIM4, TIM_IT_CC2) == SET)
  {
    TIM_ClearITPendingBit(TIM4, TIM_IT_CC2);
  }
  if(TIM_GetITStatus(TIM4, TIM_IT_CC3) == SET)
  {
    /* Clear TIM4 Capture compare interrupt pending bit */
    TIM_ClearITPendingBit(TIM4, TIM_IT_CC3);
    IC4ReadValue2 = TIM_GetCapture3(TIM4);
    /* Capture computation */
    Capture = ((0xFFFF*c_overflow - IC4ReadValue1) + IC4ReadValue2);
    c_overflow=0;
    IC4ReadValue1 = IC4ReadValue2;
    /* Frequency computation */
    TIM4Freq = (uint32_t) SystemCoreClock / Capture;
    flag =1;
  }
  if(TIM_GetITStatus(TIM4, TIM_IT_CC4) == SET)
  {
    TIM_ClearITPendingBit(TIM4, TIM_IT_CC4);
    if(flag == 1)
    {
      IC4ReadValue2 = TIM_GetCapture4(TIM4);
      Duty = ((0xFFFF*c_overflow - IC4ReadValue1) + IC4ReadValue2);
      flag = 0;
    }
  }
}

#endif







//------------------------------------------------------------------------------




#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t* file, uint32_t line)
{
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

  /* Infinite loop */
  while (1)
  {
  }
}
#endif

